/*
 * InputNode.cpp
 *
 *  Created on: 2012-05-27
 *      Author: mathieu
 */


#include <ros/ros.h>
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <image_transport/subscriber_filter.h>
#include <sensor_msgs/Image.h>
#include <geometry_msgs/TwistStamped.h>
#include "semol/Sensorimotor.h"
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <cv_bridge/cv_bridge.h>
#include "MsgConversion.h"

class ImageTwistInput
{
public:
	ImageTwistInput() :
		imageWidth_(80),
		imageHeight_(60),
		rate_(10)
	{
		ros::NodeHandle nh;
		ros::NodeHandle pnh("~");

		int queue = 10;
		pnh.param("queue", queue, queue);
		pnh.param("image_width", imageWidth_, imageWidth_);
		pnh.param("image_height", imageHeight_, imageHeight_);
		pnh.param("rate", rate_, rate_);

		ros::NodeHandle rgb_nh(nh, "rgb");
		ros::NodeHandle rgb_pnh(pnh, "rgb");
		image_transport::ImageTransport rgb_it(rgb_nh);
		image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);

		image_sub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
		twist_sub_.subscribe(nh, "cmd_vel", 1);

		sync_ = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(queue), image_sub_, twist_sub_);
		sync_->registerCallback(boost::bind(&ImageTwistInput::callback, this, _1, _2));

		sensorimotor_pub_ = nh.advertise<semol::Sensorimotor>("sensorimotor",1);
	}

	void callback(
			const sensor_msgs::ImageConstPtr& image,
			const geometry_msgs::TwistStampedConstPtr & twist)
	{
		if (rate_ > 0.0)
		{
			if ( lastUpdate_ + ros::Duration(1.0/rate_) > ros::Time::now())
			{
				return;
			}
		}

		lastUpdate_ = ros::Time::now();

		if(!image->data.size())
		{
			ROS_ERROR("Image is empty...");
			return;
		}

		// Create a sensorimotor msg
		cv::Mat data;
		semol::SensorimotorPtr sm(new semol::Sensorimotor());
		sm->header = image->header;
		sm->sensors.resize(2);

		//image
		cv_bridge::CvImageConstPtr img = cv_bridge::toCvShare(image);
		sm->sensors[0].type = semol::Sensor::kTypeImage;
		if(imageWidth_ > 0 && imageHeight_ > 0)
		{
			cv::Mat resized;
			cv::resize(img->image, resized, cv::Size(imageWidth_, imageHeight_));
			fromCvMatToCvMatMsg(sm->sensors[0].matrix, resized, false);
		}
		else
		{
			fromCvMatToCvMatMsg(sm->sensors[0].matrix, img->image, false);
		}

		//twist
		sm->actuators.resize(1);
		data = cv::Mat(1, 6, CV_32F);
		data.at<float>(0) = (float)twist->twist.linear.x;
		data.at<float>(1) = (float)twist->twist.linear.y;
		data.at<float>(2) = (float)twist->twist.linear.z;
		data.at<float>(3) = (float)twist->twist.angular.x;
		data.at<float>(4) = (float)twist->twist.angular.y;
		data.at<float>(5) = (float)twist->twist.angular.z;
		sm->actuators[0].type = semol::Actuator::kTypeTwist;
		fromCvMatToCvMatMsg(sm->actuators[0].matrix, data, false);
		sm->sensors[1].type = semol::Sensor::kTypeTwist;
		sm->sensors[1].matrix = sm->actuators[0].matrix;

		sensorimotor_pub_.publish(sm);
	}

private:
	int imageWidth_;
	int imageHeight_;
	double rate_;

	ros::Time lastUpdate_;

	//inputs
	image_transport::SubscriberFilter image_sub_;
	message_filters::Subscriber<geometry_msgs::TwistStamped> twist_sub_;

	//synchronization stuff
	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, geometry_msgs::TwistStamped> MySyncPolicy;
	message_filters::Synchronizer<MySyncPolicy> * sync_;

	ros::Publisher sensorimotor_pub_;
};


int main(int argc, char** argv)
{
	ros::init(argc, argv, "image_twist_input");
	ImageTwistInput iati;

	ros::spin();

	return 0;
}
